/**
 * 开启服务：/turtle_command, 服务数据类型std_srvs/Trigger
*/

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

class CommandServer {
 public:
  CommandServer(std::string service_name, std::string topic_name) : is_publish_command_(false) {
    server_ = nh_.advertiseService(service_name, &CommandServer::CommandServerCallback, this);
    turtle_vel_pub_ = nh_.advertise<geometry_msgs::Twist>(topic_name, 10);
    ROS_INFO("ready to receive turtle command.");
  }

  ~CommandServer() {

  }

  void TurtleVelocityPublish() {
    if (is_publish_command_) {
      geometry_msgs::Twist vel_msg;
      vel_msg.linear.x = 0.5;
      vel_msg.angular.z = 0.2;
      turtle_vel_pub_.publish(vel_msg);
    }
  }

 private:
  ros::NodeHandle nh_;
  ros::ServiceServer server_;
  ros::Publisher turtle_vel_pub_;
  bool is_publish_command_;

  bool CommandServerCallback(std_srvs::Trigger::Request& req,
    std_srvs::Trigger::Response& res) {
    is_publish_command_ = !is_publish_command_;
    ROS_INFO("Publish turtle velocity command [%s]", (is_publish_command_ ? "YES" : "NO"));
    // feedback data
    res.success = true;
    res.message = "Change turtle command state!";

    return true;
  }
};


int main(int argc, char** argv) {
  ros::init(argc, argv, "turtle_command_server");

  CommandServer command_server_node("/turtle_command", "/turtle1/cmd_vel");

  ros::Rate loop_rate(10); // 10Hz  

  while (ros::ok()) {
    ros::spinOnce(); //reade callback fuction queue.
    command_server_node.TurtleVelocityPublish();
    loop_rate.sleep();
  }

  return 0;
}

/**
 * NODE:
 * cd ~/catkin_ws
 * catkin_make
 * source devel/setup.bash
 * roscore
 * rosrun turtlesim turtlesim_node
 * rosrun learn_service_comm
 * rosservice call /turtle_command  TAB key   ENTER key
*/
